from .def_str import *
from sensor_msgs.msg import NavSatFix,NavSatStatus

def Checksum( data):
        """计算NMEA语句的校验和"""
        checksum = 0
        for char in data:
            checksum ^= ord(char)
        return checksum

def UpdateGPSInfo(line,raw_gps:NavSatFix):
    """解析NMEA语句"""
    if not '*' in line: return NO_CHECKSUM,raw_gps
    if not line.startswith('$'): return FORMAT_ERROR,raw_gps
    data_parts, checksum_part = line[1:].split('*', 1)
    if Checksum(data_parts) != int(checksum_part,16): return CHECKSUM_ERROR,raw_gps
    parts=data_parts.split(',')
    if parts[0]=='GNGGA':
        '''
            基础GPS数据
        '''
        utc_time=parts[1]   # UTC标准时间
        latitute=float(parts[2][0:2])+float(parts[2][2:])/60.0 
        if parts[3]=='S': latitute=-latitute    #纬度信息
        longitute=float(parts[4][0:3])+float(parts[4][3:])/60.0 
        if parts[5]=='W': longitute=-longitute    #纬度信息
        loc_status=int(parts[6]) # 定位状态 1-GPS 4-差分固定 5-差分浮点
        satellites=int(parts[7]) #卫星数量
        HDOP=float(parts[8])   #精度因子（越小越好，关联协方差）
        sea_altitute=float(parts[9]) #海拔高度
        lla_altitute=float(parts[11])#大地水准高度
        try:
            rtk_latency=float(parts[13])   #RTK基站延时
            rtk_id=int(parts[14])       #RTK基站ID
        except:
             pass

        '''封装传入的原始GPS信息'''
        
        raw_gps.longitude=longitute
        raw_gps.latitude=latitute
        if loc_status==4: raw_gps.status.status=2
        elif loc_status in [1,2,3]: raw_gps.status.status=0
        elif loc_status==5: raw_gps.status.status=1
        else: raw_gps.status.status=-1
        raw_gps.status.service=satellites
        raw_gps.position_covariance[0]=HDOP
        
        return 'GNGGA',raw_gps
        
    elif parts[0]=='GNRMC':
        '''
            推荐GPS数据
        '''
        utc_time=parts[1]   # UTC标准时间
        loc_valid=False
        if parts[2]=='A': loc_valid=True    #定位是否有效
        latitute=float(parts[3][0:2])+float(parts[3][2:])/60.0 
        if parts[4]=='S': latitute=-latitute    #纬度信息
        longitute=float(parts[5][0:3])+float(parts[5][3:])/60.0 
        if parts[6]=='W': longitute=-longitute    #纬度信息
        speed=float(parts[7])*1.852/3.6         #海里/小时 转化为米/秒
        speed_angle=float(parts[8])             #速度正北方向的旋转角
        date="20"+parts[9][4:6]+"-"+parts[9][2:4]+"-"+parts[9][0:2]
        return 'GNRMC',raw_gps
    else: 
        return 'GNUTILS',raw_gps



